/**
 * @Author Yunfeng Bai
 *
 * 2010-10-30
 */
package util;

import javax.vecmath.Matrix3d;
import javax.vecmath.Matrix4d;
import javax.vecmath.Matrix4f;
import javax.vecmath.Point3d;
import javax.vecmath.Quat4d;
import javax.vecmath.Tuple3d;
import javax.vecmath.Vector3d;
import javax.vecmath.Vector3f;
import javax.vecmath.Vector4d;
import javax.vecmath.Vector4f;

import cern.colt.matrix.DoubleMatrix1D;
import cern.colt.matrix.DoubleMatrix2D;
import cern.colt.matrix.linalg.EigenvalueDecomposition;

public class Util
{
	public static boolean OUTPUT_DEBUG_INFO = true;
	
	public static double min(double a, double b, double c)
	{
		return Math.min(Math.min(a, b), c);
	}
			
    public static double max(double a, double b, double c)
    {
        return Math.max(Math.max(a, b), c);
    }

    public static float max(float a, float b, float c)
    {
        return Math.max(Math.max(a, b), c);
    }

    public static Vector3f toVec3(Vector4f v)
    {
        Vector3f r = new Vector3f(v.x, v.y, v.z);
        return r;
    }

    public static Vector4f toVec4(Vector3f v, double w)
    {
        Vector4f r = new Vector4f(v.x, v.y, v.z, (float) w);
        return r;
    }

    public static Vector3d toVec3(Vector4d v)
    {
        Vector3d r = new Vector3d(v.x, v.y, v.z);
        return r;
    }

    public static Vector4d toVec4(Vector3d v, double w)
    {
        Vector4d r = new Vector4d(v.x, v.y, v.z, w);
        return r;
    }

    public static Vector4d sub(Vector4d a, Vector4d b)
    {
        Vector4d c = new Vector4d(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
        return c;
    }
    
    public static Vector3d sub(Point3d a, Point3d b)
    {
    	Vector3d c = new Vector3d(a.x - b.x, a.y - b.y, a.z - b.z);
    	return c;
    }

    public static Vector4d add(Vector4d a, Vector4d b)
    {
        Vector4d c = new Vector4d(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
        return c;
    }

    public static Vector3d sub(Vector3d a, Vector3d b)
    {
        Vector3d c = new Vector3d(a.x - b.x, a.y - b.y, a.z - b.z);
        return c;
    }

    public static Vector3d add(Vector3d a, Vector3d b)
    {
        Vector3d c = new Vector3d(a.x + b.x, a.y + b.y, a.z + b.z);
        return c;
    }
    
    public static Point3d add(Point3d a, Vector3d b)
    {
    	Point3d c = new Point3d(a.x + b.x, a.y + b.y, a.z + b.z);
        return c;
    }

    public static Vector4f sub(Vector4f a, Vector4f b)
    {
        Vector4f c = new Vector4f(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
        return c;
    }

    public static Vector4f add(Vector4f a, Vector4f b)
    {
        Vector4f c = new Vector4f(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
        return c;
    }

    public static Vector3f sub(Vector3f a, Vector3f b)
    {
        Vector3f c = new Vector3f(a.x - b.x, a.y - b.y, a.z - b.z);
        return c;
    }

    public static Vector3f add(Vector3f a, Vector3f b)
    {
        Vector3f c = new Vector3f(a.x + b.x, a.y + b.y, a.z + b.z);
        return c;
    }

    public static Vector3f cross(Vector3f a, Vector3f b)
    {
        Vector3f c = new Vector3f();
        c.cross(a, b);
        return c;
    }
    
    public static Vector3d cross(Vector3d a, Vector3d b)
    {
        Vector3d c = new Vector3d();
        c.cross(a, b);
        return c;
    }

    public static Vector4f scale(Vector4f a, double s)
    {
        Vector4f c = (Vector4f) a.clone();
        c.scale((float) s);
        return c;
    }

    public static Vector3f scale(Vector3f a, double s)
    {
        Vector3f c = (Vector3f) a.clone();
        c.scale((float) s);
        return c;
    }

    public static Vector4d scale(Vector4d a, double s)
    {
        Vector4d c = (Vector4d) a.clone();
        c.scale((float) s);
        return c;
    }

    public static Vector3d scale(Vector3d a, double s)
    {
        Vector3d c = (Vector3d) a.clone();
        c.scale((float) s);
        return c;
    }
    
    public static Point3d scale(Point3d a, double s)
    {
    	Point3d c = (Point3d) a.clone();
        c.scale((float) s);
        return c;
    }

    public static float[] matrixToArray(Matrix4f m)
    {
        float[] result =
        { m.m00, m.m10, m.m20, m.m30, m.m01, m.m11, m.m21, m.m31, m.m02, m.m12,
                m.m22, m.m32, m.m03, m.m13, m.m23, m.m33 };
        return result;
    }

    public static double[] matrixToArray(Matrix4d m)
    {
        double[] result =
        { m.m00, m.m10, m.m20, m.m30, m.m01, m.m11, m.m21, m.m31, m.m02, m.m12,
                m.m22, m.m32, m.m03, m.m13, m.m23, m.m33 };
        return result;
    }
    
    public static double[] axesToArray(Vector3d[] axes)
    {
    	double[] result =
    		{axes[0].x, axes[0].y, axes[0].z, 0,
		 	 axes[1].x, axes[1].y, axes[1].z, 0,
	 		 axes[2].x, axes[2].y, axes[2].z, 0,
 			 0, 0, 0, 1};
    	return result;
    }

    public static Matrix4f arrayToMatrix(float[] m)
    {
        Matrix4f result = new Matrix4f(m[0], m[4], m[8], m[12], m[1], m[5],
                m[9], m[13], m[2], m[6], m[10], m[14], m[3], m[7], m[11], m[15]);
        return result;
    }

    public static Matrix4f getIdentity()
    {
        Matrix4f temp = new Matrix4f();
        temp.setIdentity();
        return temp;
    }

    public static Vector3f multiplyTMatrix4fAndVector3f(Matrix4f m, Vector3f v,
            int w)
    {
        float x = m.m00 * v.x + m.m01 * v.y + m.m02 * v.z + m.m03 * w;
        float y = m.m10 * v.x + m.m11 * v.y + m.m12 * v.z + m.m13 * w;
        float z = m.m20 * v.x + m.m21 * v.y + m.m22 * v.z + m.m23 * w;

        return new Vector3f(x, y, z);
    }

    /**
     * This method comes from the pixar lecture notes.
     */
    public static Matrix4d getMatrixFromQuaternion(Quat4d q)
    {
        Matrix4d m = new Matrix4d();
        double s = q.w, vx = q.x, vy = q.y, vz = q.z;
        m.m00 = 1 - 2 * vy * vy - 2 * vz * vz;
        m.m01 = 2 * vx * vy - 2 * s * vz;
        m.m02 = 2 * vx * vz + 2 * s * vy;
        m.m10 = 2 * vx * vy + 2 * s * vz;
        m.m11 = 1 - 2 * vx * vx - 2 * vz * vz;
        m.m12 = 2 * vy * vz - 2 * s * vx;
        m.m20 = 2 * vx * vz - 2 * s * vy;
        m.m21 = 2 * vy * vz + 2 * s * vx;
        m.m22 = 1 - 2 * vx * vx - 2 * vy * vy;
        m.m33 = 1;
        return m;
    }

    public static Matrix3d toMatrix3d(Matrix4d m)
    {
        Matrix3d m3 = new Matrix3d();
        m3.m00 = m.m00;
        m3.m01 = m.m01;
        m3.m02 = m.m02;
        m3.m10 = m.m10;
        m3.m11 = m.m11;
        m3.m12 = m.m12;
        m3.m20 = m.m20;
        m3.m21 = m.m21;
        m3.m22 = m.m22;
        return m3;
    }
    
    public static double distanceSqr(Tuple3d a, Tuple3d b)
    {
    	double x = a.x - b.x;
    	double y = a.y - b.y;
    	double z = a.z - b.z;
    	return x * x + y * y + z * z;
    }
    
    public static double distance(Tuple3d a, Tuple3d b)
    {
    	double x = a.x - b.x;
    	double y = a.y - b.y;
    	double z = a.z - b.z;
    	return Math.sqrt(x * x + y * y + z * z);
    }
    
    public static Matrix4f getViewMatrix(Vector3f pos, Vector3f targetDir, Vector3f up)
	{
		Vector3f z = new Vector3f(targetDir);
		z.normalize();
		z.negate();
		Vector3f x = new Vector3f(up);
		x.normalize();
		x.cross(x, z);
		x.normalize();
		Vector3f y = new Vector3f();
		y.cross(z, x);
		y.normalize();
		
		Matrix4f rotation = new Matrix4f();
		rotation.setRow(0, x.x, x.y, x.z, 0);
		rotation.setRow(1, y.x, y.y, y.z, 0);
		rotation.setRow(2, z.x, z.y, z.z, 0);
		rotation.m33 = 1;
		
		Matrix4f translation = new Matrix4f();
		translation.setIdentity();
		translation.setColumn(3, -pos.x, -pos.y, -pos.z, 1);
		
		Matrix4f viewMatrix = new Matrix4f();
		viewMatrix.mul(rotation, translation);
		
		return viewMatrix;
	}
	
	/**
	 * @param l Left
	 * @param r Right
	 * @param b Bottom
	 * @param t Top
	 * @param n Near
	 * @param f Far
	 * @return
	 */
	public static Matrix4f getOrthogonalProjectionMatrix(float l, float r, float b, float t, float n, float f)
	{
		Matrix4f matrix = new Matrix4f();
		matrix.m00 = 2.0f / (r - l);
		matrix.m03 = (r + l) / (l - r);
		matrix.m11 = 2.0f / (t - b);
		matrix.m13 = (t + b) / (b - t);
		matrix.m22 = 2.0f / (n - f);
		matrix.m23 = (n + f) / (n - f);
		matrix.m33 = 1;
		
		return matrix;
	}
	
	public static float[] toFloatArray(double[] a)
	{
		float[] f = new float[a.length];
		for (int i = 0; i < a.length; i++)
			f[i] = (float) a[i];
		return f;
	}
	
	public static double getA(Tuple3d p, int axis) 
	{
		if (axis == 0) return p.x;
		else if (axis == 1) return p.y;
		else return p.z;
	}
	
	public static void setA(Tuple3d p, int axis, double v)
	{
		if (axis == 0) p.x = v;
		else if (axis == 1) p.y = v;
		else p.z = v;
	}
	
	public static double dot(Tuple3d a, Tuple3d b)
	{
		return a.x * b.x + a.y * b.y + a.z * b.z;
	}
	
	public static void traceTime(String message, long time)
	{
		if (OUTPUT_DEBUG_INFO)
			System.out.println(message + " " + (time / 1000000.0) + "ms");
	}
	
	public static void trace(String message)
	{
		if (OUTPUT_DEBUG_INFO)
			System.out.println(message);
	}
	
	public static boolean checkSemiPositiveDefinite(DoubleMatrix2D A)
	{
		EigenvalueDecomposition ed = new EigenvalueDecomposition(A);
		DoubleMatrix1D eigenvalues = ed.getRealEigenvalues();
		for (int i = 0; i < eigenvalues.size(); i++)
		{
			if (eigenvalues.get(i) < 0)
			{
				return false;
			}
		}
		return true;
	}
	
	public static boolean checkSymmetric(DoubleMatrix2D a)
	{
		int n = a.rows();
		for (int i = 0; i < n; i++)
			for (int j = 0; j < i; j++)
				if (a.get(i, j) != a.get(j, i))
				{
					return false;
				}
		return true;
	}
}
